In [1]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
import os
import glob
from pprint import pprint

Helpers

In [2]:
def draw_lines(img, lines, color=[255, 0, 0], thickness=2):
    for line in lines:
        x1,y1,x2,y2 = line
        cv2.line(img, (x1, y1), (x2, y2), color, thickness)

def draw_vertices(img, vertices, color=[255, 0, 0], thickness=2):
    v = np.copy(vertices)
    v = np.concatenate((v, [v[0]]), axis=0)
    lines = [(v[i][0], v[i][1], v[i+1][0], v[i+1][1]) for i in range(len(v)-1)]  
    draw_lines(img, lines, color, thickness)

Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.

In [3]:
# prepare object points
nx = 9
ny = 6

def getCalibrationPoints(images):
    
    # Arrays to store object points and image points from all the images.
    objpoints = [] # 3d point in real world space
    imgpoints = [] # 2d points in image plane.

    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((nx*ny,3), np.float32)
    objp[:,:2] = np.mgrid[0:nx,0:ny].T.reshape(-1,2)

    for fname in images:

        img = cv2.imread(fname)

        # Convert to grayscale
        gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

        # Find the chessboard corners
        ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)

        # If save imgpoints
        if ret == True:
            imgpoints.append(corners)
            objpoints.append(objp)
            
        img_shape = img.shape
            
    return imgpoints, objpoints, img_shape

def getCalibrationMatrixAndDistortionCoefficients(images):
    imgpoints, objpoints, img_shape = getCalibrationPoints(images)
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
            objpoints, 
            imgpoints,
            img_shape[1::-1],
            None,
            None
            )
    
    return mtx, dist

# Test
cal_images = glob.glob('camera_cal/*.jpg')
mtx, dist = getCalibrationMatrixAndDistortionCoefficients(cal_images)

# draw corners
img = cv2.imread(cal_images[13])
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
cv2.drawChessboardCorners(img, (nx, ny), corners, ret)

# undistort
dst = cv2.undistort(img, mtx, dist, None, mtx)
bgr = cv2.cvtColor(dst, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/calibration_dist_correction.jpg', bgr)
    
# plot
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(dst)
ax2.set_title('Undistorted Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Apply a distortion correction to raw images.

Save corrected images in output_images prefixed with dist_correction_

In [4]:
raw_images = os.listdir("test_images/")
no_of_images = len(raw_images)
for i in range(no_of_images):
    img = mpimg.imread("test_images/" + raw_images[i])
    dst = cv2.undistort(img, mtx, dist, None, mtx)
    
    bgr = cv2.cvtColor(dst, cv2.COLOR_RGB2BGR)
    cv2.imwrite('output_images/dist_correction_' + raw_images[i], bgr)
    
    # plot
    fig = plt.figure(figsize=(24, 80))
    fig.tight_layout()
    fig.add_subplot(no_of_images,2,1 + i*2)
    plt.imshow(img)
    fig.add_subplot(no_of_images,2,2 + i*2)
    plt.imshow(dst)

Use color transforms, gradients, etc., to create a thresholded binary image.

In [5]:
# Define a function that applies Sobel x or y, 
# then takes an absolute value and applies a threshold.
# Note: calling your function with orient='x', thresh_min=5, thresh_max=100
# should produce output like the example image shown above this quiz.
def abs_sobel_thresh(gray, orient='x', sobel_kernel=3, thresh=(0, 255)):
    
    # Apply the following steps to img
    # 2) Take the derivative in x or y given orient = 'x' or 'y'
    if orient == 'x':
        sobel = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    if orient == 'y':
        sobel = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # 3) Take the absolute value of the derivative or gradient
    abs_sobel = np.absolute(sobel)
    # 4) Scale to 8-bit (0 - 255) then convert to type = np.uint8
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # 5) Create a mask of 1's where the scaled gradient magnitude 
            # is > thresh_min and < thresh_max
    binary_output = np.zeros_like(scaled_sobel)
    binary_output[(scaled_sobel >= thresh[0]) & (scaled_sobel <= thresh[1])] = 1
    # 6) Return this mask as your binary_output image
    return binary_output

# Define a function that applies Sobel x and y, 
# then computes the magnitude of the gradient
# and applies a threshold
def mag_threshold(gray, sobel_kernel=3, mag_thresh=(0, 255)):
    
    # Apply the following steps to img
    # 2) Take the gradient in x and y separately
    sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # 3) Calculate the magnitude 
    abs_sobel= np.sqrt(sobel_x**2 + sobel_y**2)
    # 4) Scale to 8-bit (0 - 255) and convert to type = np.uint8
    scaled_sobel = np.uint8(255*abs_sobel/np.max(abs_sobel))
    # 5) Create a binary mask where mag thresholds are met
    binary_output = np.zeros_like(scaled_sobel)
    binary_output[(scaled_sobel >= mag_thresh[0]) & (scaled_sobel <= mag_thresh[1])] = 1
    # 6) Return this mask as your binary_output image
    return binary_output

# Define a function that applies Sobel x and y, 
# then computes the direction of the gradient
# and applies a threshold.
def dir_threshold(gray, sobel_kernel=3, dir_thresh=(0, np.pi/2)):
    # Apply the following steps to img
    # 2) Take the gradient in x and y separately
    sobel_x = cv2.Sobel(gray, cv2.CV_64F, 1, 0, ksize=sobel_kernel)
    sobel_y = cv2.Sobel(gray, cv2.CV_64F, 0, 1, ksize=sobel_kernel)
    # 3) Take the absolute value of the x and y gradients
    abs_sobelx = np.abs(sobel_x) 
    abs_sobely = np.abs(sobel_y) 
    # 4) Use np.arctan2(abs_sobely, abs_sobelx) to calculate the direction of the gradient 
    direction = np.arctan2(abs_sobely, abs_sobelx)
    # 5) Create a binary mask where direction thresholds are met
    binary_output = np.zeros_like(direction)
    binary_output[(direction >= dir_thresh[0]) & (direction <= dir_thresh[1])] = 1
    # 6) Return this mask as your binary_output image
    return binary_output

def thresholds(gray, kernel=3, thresh=(0, 255)):
    blur = cv2.blur(gray, (kernel, kernel))
    binary = np.zeros_like(blur)
    binary[(blur >= thresh[0]) & (blur <= thresh[1])] = 1
    
    return binary

def region_of_interest(img, vertices):
    """
    Applies an image mask.
    
    Only keeps the region of the image defined by the polygon
    formed from `vertices`. The rest of the image is set to black.
    `vertices` should be a numpy array of integer points.
    """
    #defining a blank mask to start with
    mask = np.zeros_like(img)   
    
    #defining a 3 channel or 1 channel color to fill the mask with depending on the input image
    if len(img.shape) > 2:
        channel_count = img.shape[2]  # i.e. 3 or 4 depending on your image
        ignore_mask_color = (255,) * channel_count
    else:
        ignore_mask_color = 255
        
    #filling pixels inside the polygon defined by "vertices" with the fill color    
    cv2.fillPoly(mask, vertices, ignore_mask_color)
    
    #returning the image only where mask pixels are nonzero
    masked_image = cv2.bitwise_and(img, mask)
    return masked_image
                
def combining_thresholds(img, kernel=3, 
                         x_thresh=(0, 255), 
                         y_thresh=(0, 255), 
                         mag_thresh=(0, 255), 
                         dir_thresh=(0, np.pi/2),
                         s_thresh=(0, 255)
                        ):
    
    # Convert to HLS color space and separate the V channel
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS)
    l_channel = hls[:,:,1]
    s_channel = hls[:,:,2]
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
    
    # Apply each of the thresholding functions
    gradx = abs_sobel_thresh(l_channel, orient='x', sobel_kernel=kernel, thresh=x_thresh)
    grady = abs_sobel_thresh(l_channel, orient='y', sobel_kernel=kernel, thresh=y_thresh)
    #mag_binary = mag_threshold(l_channel, sobel_kernel=kernel, mag_thresh=mag_thresh)
    #dir_binary = dir_threshold(l_channel, sobel_kernel=kernel, dir_thresh=dir_thresh)
    s_binary = thresholds(s_channel, kernel=kernel, thresh=s_thresh)

    combined = np.zeros_like(gradx)
  
    combined[
        ((gradx == 1) & (grady == 1)) | 
        #((mag_binary == 1) & (dir_binary == 1)) |
        s_binary == 1
    ] = 1

    #combined = np.dstack((s_binary, (mag_binary == 1) & (dir_binary == 1), (gradx == 1) & (grady == 1))) * 255

    return combined
In [6]:
cor_images = glob.glob('output_images/dist_correction_*.jpg')
cor_images.sort()
no_of_images = len(cor_images)

def getVerticesRoi(img):
    # roi
    top_roi = 450
    top_width_roi = 160
    left_roi = 170
    right_roi = 140
    bottom_roi = 20

    image_size = img.shape[1::-1]
    return np.array([[
            (left_roi, image_size[1]-bottom_roi),
            (image_size[0]/2-top_width_roi/2, top_roi), 
            (image_size[0]/2+top_width_roi/2, top_roi), 
            (image_size[0]-right_roi, image_size[1]-bottom_roi)
        ]], dtype=np.int32)
  
img = mpimg.imread(cor_images[0])
vertices_roi = getVerticesRoi(img)

for i in range(no_of_images):
#for i in [0]:
    img = mpimg.imread(cor_images[i])
        
    dst = combining_thresholds(
        img, 
        kernel=5, 
        x_thresh=(30, 255),
        y_thresh=(30, 255),
        mag_thresh=(50, 255),
        dir_thresh=(0.1, 1.4),
        s_thresh=(140, 255)
    )
    
    dst = region_of_interest(dst, vertices_roi)

    color_binary = np.dstack((dst, dst, dst)) * 255
    fname = cor_images[i].replace('dist_correction_', 'binary_');
    cv2.imwrite(fname, color_binary)
    
    # plot
    fig = plt.figure(figsize=(24, 80))
    fig.tight_layout()
    fig.add_subplot(no_of_images,2,1)
    x = vertices_roi[:,:,0].flatten()
    y = vertices_roi[:,:,1].flatten()
    plt.plot(x, y, 'b--', lw=1)
    plt.imshow(img)
    fig.add_subplot(no_of_images,2,2)
    plt.imshow(dst, cmap='gray')
    #plt.imshow(dst)

Apply a perspective transform to rectify binary image

In [7]:
src_vertices = np.float32([
        [590, 452],
        [690, 452], 
        [1120, 720], 
        [194, 720]
    ])

dst_vertices = np.float32([
        [294, 0],
        [1020, 0], 
        [1020, 720], 
        [294, 720]
    ])

M = cv2.getPerspectiveTransform(src_vertices, dst_vertices)
Minv = cv2.getPerspectiveTransform(dst_vertices, src_vertices)

img = mpimg.imread(cor_images[0])
img_size = img.shape[1::-1]

warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)

draw_vertices(img, src_vertices, thickness=2)
draw_vertices(warped, dst_vertices, thickness=2)

# plot
fig = plt.figure(figsize=(80, 80))
fig.add_subplot(111)
plt.imshow(img)

bgr = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/perspective_image.jpg', bgr)
bgr = cv2.cvtColor(warped, cv2.COLOR_RGB2BGR)
cv2.imwrite('output_images/perspective_warped.jpg', bgr)
        
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
f.tight_layout()
ax1.imshow(img)
ax1.set_title('Original Image', fontsize=50)
ax2.imshow(warped)
ax2.set_title('Transform Image', fontsize=50)
plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)


bin_images = glob.glob('output_images/binary_*.jpg')
bin_images.sort()
no_of_images = len(bin_images)

for i in range(no_of_images):
#for i in [0]:
    img = mpimg.imread(bin_images[i])
    warped = cv2.warpPerspective(img, M, img_size, flags=cv2.INTER_LINEAR)
    
    fname = bin_images[i].replace('binary_', 'warped_');
    cv2.imwrite(fname, warped)
    
    draw_vertices(img, src_vertices, thickness=2)
    draw_vertices(warped, dst_vertices, thickness=2)
    
    # plot
    f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
    f.tight_layout()
    ax1.imshow(img)
    ax2.imshow(warped)
    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Detect lane pixels and fit to find the lane boundary

Finding the Lines: Sliding Window

In [8]:
# Presumption 
# - the projection of the lane is about 30 meters long
# - the lane is about 3.7 meters wide
# Define conversions in x and y from pixels space to meters
xm_per_pix = 3.7/726 # meters per pixel in x dimension (1020 - 294 = 726) 
ym_per_pix = 30.0/720 # meters per pixel in y dimension 

class Line():
    def __init__(self, shape, xm_per_pix, ym_per_pix):
        
        # max last n lines saved
        self.max_n_lines = 5
        
        # was the line detected in the last iteration?
        self.detected = False  
        # x values of the last n fits of the line
        self.recent_xfitted = [] 
        #average x values of the fitted line over the last n iterations
        self.bestx = None 
        # the last n fits of the line
        self.recent_fitted = []
        #polynomial coefficients averaged over the last n iterations
        self.best_fit = None  
        #polynomial coefficients for the most recent fit
        self.current_fit = [np.array([False])]  
        #radius of curvature of the line in some units
        self.radius_of_curvature = None 
        #distance in meters of vehicle center from the line
        self.line_base_pos = None 
        #difference in fit coefficients between last and new fits
        self.diffs = np.array([0,0,0], dtype='float') 
        #x values for detected line pixels
        self.allx = None  
        #y values for detected line pixels
        self.ally = None  
        
        # the shape of the line image
        self.shape = shape
        # meters per pixel in y dimension
        self.ym_per_pix = ym_per_pix
        # meters per pixel in x dimension
        self.xm_per_pix = xm_per_pix
        # bottom of your image
        self.bottom_y = self.shape[0] - 1
        
    def props(self):
        return vars(self)
        
    def init(self, allx, ally):
        self.fitPolynomial(allx, ally)
        self.updateLineBasePos()
        self.updateRadiusOfCurvature()
        
    # calculate current_fit
    def fitPolynomial(self, allx, ally):
        line_detected = len(allx) != 0
        
        # Fit new polynomials
        fit = None
        bestx = None
        recent_xfitted = []
        recent_fitted = []
        if line_detected == True:    
            fit = np.polyfit(ally, allx, 2)
            y_eval = self.bottom_y
            bestx = fit[0]*y_eval**2 + fit[1]*y_eval + fit[2]
            recent_xfitted = [bestx]
            recent_fitted = [fit]
            
        self.detected = line_detected
        self.allx = allx
        self.ally = ally
        self.current_fit = fit
        self.best_fit = fit
        self.bestx = bestx
        self.recent_xfitted = recent_xfitted
        self.recent_fitted = recent_fitted
           
    def append(self, line):
        if line.detected is False:
            raise Exception('do not append an undetected line')
        
        newLine = Line(self.shape, self.xm_per_pix, self.ym_per_pix)
      
        newLine.__dict__.update(self.__dict__)
        
        newLine.recent_xfitted = list(newLine.recent_xfitted)
        newLine.recent_xfitted.append(line.bestx)
        if len(newLine.recent_xfitted) > self.max_n_lines:
            newLine.recent_xfitted = newLine.recent_xfitted[1:]
        
        newLine.bestx = np.average(newLine.recent_xfitted)
        
        newLine.recent_fitted = list(newLine.recent_fitted)
        newLine.recent_fitted.append(line.best_fit)
        if len(newLine.recent_fitted) > self.max_n_lines:
            newLine.recent_fitted = newLine.recent_fitted[1:]
            
        newLine.current_fit = line.current_fit
        newLine.best_fit = np.average(newLine.recent_fitted, axis=0)
        newLine.diffs = newLine.best_fit - line.best_fit
   
        newLine.allx = line.allx
        newLine.ally = line.ally
        
        newLine.updateLineBasePos()
        newLine.updateRadiusOfCurvature()
        
        return newLine
        
    # use best_fit to estimate line_base_pos
    def updateLineBasePos(self):
        if self.best_fit is None:
            return
        
        # calculate line_base_pos
        y_eval = self.bottom_y
        fitx = self.fitX(y_eval)
        self.line_base_pos = abs(fitx - self.shape[1]/2)*self.xm_per_pix
        
    # use best_fit to estimate radius_of_curvature
    def updateRadiusOfCurvature(self):
        if self.best_fit is None:
            return
        
        # scale polynomial coefficients A and B to fit in meters
        a_scale = self.xm_per_pix/(self.ym_per_pix**2)
        b_scale = self.xm_per_pix/self.ym_per_pix
        
        # calculate radius_of_curvature
        y_eval = self.bottom_y
        numerator = ((1 + (2*a_scale*self.best_fit[0]*y_eval*self.ym_per_pix + b_scale*self.best_fit[1])**2)**1.5)
        denominator = np.absolute(2*a_scale*self.best_fit[0])
        
        self.radius_of_curvature = numerator/denominator
    
    # Generate x and y values for plotting
    def generateXY(self, shape):
        ploty = np.linspace(0, shape[0]-1, shape[0])
            
        return self.fitX(ploty), ploty
    
    # use best_fit to fit x from y
    def fitX(self, ploty):
        try:
            fitx = self.best_fit[0]*ploty**2 + self.best_fit[1]*ploty + self.best_fit[2]
        except TypeError:
            # Avoids an error if `left` and `right_fit` are still none or incorrect
            # print('The function failed to fit a line!')
            fitx = 1*ploty**2 + 1*ploty
            
        return fitx
    
def find_lane_pixels(binary_warped):
    # Take a histogram of the bottom half of the image
    histogram = np.sum(binary_warped[binary_warped.shape[0]//2:,:], axis=0)

    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines
    midpoint = np.int(histogram.shape[0]//2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint

    # HYPERPARAMETERS
    # Choose the number of sliding windows
    nwindows = 9
    # Set the width of the windows +/- margin
    margin = 100
    # Set minimum number of pixels found to recenter window
    minpix = 60

    # Set height of windows - based on nwindows above and image shape
    window_height = np.int(binary_warped.shape[0]//nwindows)
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # Current positions to be updated later for each window in nwindows
    leftx_current = leftx_base
    rightx_current = rightx_base

    # Create empty lists to receive left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []

    # Step through the windows one by one
    for window in range(nwindows):
        # Identify window boundaries in x and y (and right and left)
        win_y_low = binary_warped.shape[0] - (window+1)*window_height
        win_y_high = binary_warped.shape[0] - window*window_height
        ### TO-DO: Find the four below boundaries of the window ###
        win_xleft_low = leftx_current - margin  # Update this
        win_xleft_high = leftx_current + margin  # Update this
        win_xright_low = rightx_current - margin  # Update this
        win_xright_high = rightx_current + margin  # Update this
        
        
        ### TO-DO: Identify the nonzero pixels in x and y within the window ###
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
        (nonzerox >= win_xleft_low) &  (nonzerox < win_xleft_high)).nonzero()[0]
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
        (nonzerox >= win_xright_low) &  (nonzerox < win_xright_high)).nonzero()[0]
        
        # Append these indices to the lists
        left_lane_inds.append(good_left_inds)
        right_lane_inds.append(good_right_inds)
        
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
            
        if len(good_right_inds) > minpix:
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))

    # Concatenate the arrays of indices (previously was a list of lists of pixels)
    try:
        left_lane_inds = np.concatenate(left_lane_inds)
        right_lane_inds = np.concatenate(right_lane_inds)
    except ValueError:
        # Avoids an error if the above is not implemented fully
        pass

    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds] 
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]

    return leftx, lefty, rightx, righty

def fitSlidingWindow(binary_warped):
    # Find our lane pixels first
    leftx, lefty, rightx, righty = find_lane_pixels(binary_warped)
    
    shape = binary_warped.shape
    leftLane = Line(shape, xm_per_pix, ym_per_pix)
    leftLane.init(leftx, lefty)
    
    rightLane = Line(shape, xm_per_pix, ym_per_pix)
    rightLane.init(rightx, righty)

    return leftLane, rightLane

def drawLane(image, warped, Minv, leftLane, rightLane):
    
    left_fitx, left_ploty = leftLane.generateXY(warped.shape)
    right_fitx, right_ploty = rightLane.generateXY(warped.shape)
    
    # Create an image to draw the lines on
    warp_zero = np.zeros_like(warped).astype(np.uint8)
    color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

    # Recast the x and y points into usable format for cv2.fillPoly()
    pts_left = np.array([np.transpose(np.vstack([left_fitx, left_ploty]))])
    pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, right_ploty])))])
    pts = np.hstack((pts_left, pts_right))

    # Draw the lane onto the warped blank image
    cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))
    
    newwarp = np.dstack((warped, warped, warped))
    lane_warped = cv2.addWeighted(newwarp, 1, color_warp, 0.3, 0)
    
    # Warp the blank back to original image space using inverse perspective matrix (Minv)
    newwarp = cv2.warpPerspective(color_warp, Minv, (image.shape[1], image.shape[0])) 
    # Combine the result with the original image
    
    lane_image = cv2.addWeighted(image, 1, newwarp, 0.3, 0)
    
    return lane_image, lane_warped

Warp the detected lane boundaries back onto the original image.

In [9]:
warped_images = glob.glob('output_images/warped_*.jpg')
warped_images.sort()
no_of_images = len(warped_images)

sliding_lanes = []

for i in range(no_of_images):
#for i in [0]:
    img = mpimg.imread(warped_images[i])
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    leftLane, rightLane = fitSlidingWindow(gray)
    sliding_lanes.append((leftLane, rightLane))
    
    cor_image = mpimg.imread(cor_images[i])
    lane_image, lane_warped = drawLane(cor_image, gray, Minv, leftLane, rightLane)
    
    # plot
    f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
    f.tight_layout()
    ax1.imshow(lane_warped)
    ax2.imshow(lane_image)
    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
    

Finding the Lines: Search from Prior

In [10]:
def search_around_poly(binary_warped, left_fit, right_fit):
    # HYPERPARAMETER
    # Choose the width of the margin around the previous polynomial to search
    margin = 50

    # Grab activated pixels
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    
    left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + 
                    left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + 
                    left_fit[1]*nonzeroy + left_fit[2] + margin)))
    right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + 
                    right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + 
                    right_fit[1]*nonzeroy + right_fit[2] + margin)))
    
    # Again, extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds] 
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    
    return leftx, lefty, rightx, righty

def fitAroundPoly(binary_warped, leftLane, rightLane):
    leftx, lefty, rightx, righty = search_around_poly(binary_warped, leftLane.best_fit, rightLane.best_fit)
    
    shape = binary_warped.shape
    leftLane = Line(shape, xm_per_pix, ym_per_pix)
    leftLane.init(leftx, lefty)
    
    rightLane = Line(shape, xm_per_pix, ym_per_pix)
    rightLane.init(rightx, righty)

    return leftLane, rightLane

Warp the detected lane boundaries back onto the original image.

In [11]:
prior_lanes = []

for i in range(no_of_images):
#for i in [0]:
    img = mpimg.imread(warped_images[i])
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    leftOldLane, rightOldLane = sliding_lanes[i]
    leftLane, rightLane = fitAroundPoly(gray, leftOldLane, rightOldLane)
    prior_lanes.append((leftLane, rightLane))

    cor_image = mpimg.imread(cor_images[i])
    lane_image, lane_warped = drawLane(cor_image, gray, Minv, leftLane, rightLane)
    
    fname = warped_images[i].replace('warped_', 'lane_warped_');
    cv2.imwrite(fname, lane_warped)
    
    fname = warped_images[i].replace('warped_', 'lane_image_');
    bgr_lane_image = cv2.cvtColor(lane_image, cv2.COLOR_RGB2BGR)
    cv2.imwrite(fname, bgr_lane_image)
    
    # plot
    f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
    f.tight_layout()
    ax1.imshow(lane_warped)
    ax2.imshow(lane_image)
    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)

Determine the curvature of the lane and vehicle position with respect to center

Curvature of the lane and vehicle position are both implemented as methods on the Line class. xm_per_pix and ym_per_pix Define conversions in x and y from pixels space to meters. The constants a_scale and b_scale is needed to convert x and y from pixels space to meters.

Presumption

  • the projection of the lane is about 30 meters long
  • the lane is about 3.7 meters wide
# use best_fit to estimate line_base_pos
def updateLineBasePos(self):
    if self.best_fit is None:
        return

    # calculate line_base_pos
    y_eval = self.bottom_y
    fitx = self.fitX(y_eval)
    self.line_base_pos = abs(fitx - self.shape[1]/2)*self.xm_per_pix

# use best_fit to estimate radius_of_curvature
def updateRadiusOfCurvature(self):
    if self.best_fit is None:
        return

    # scale polynomial coefficients A and B to fit in meters
    a_scale = self.xm_per_pix/(self.ym_per_pix**2)
    b_scale = self.xm_per_pix/self.ym_per_pix

    # calculate radius_of_curvature
    y_eval = self.bottom_y
    numerator = ((1 + (2*a_scale*self.best_fit[0]*y_eval*self.ym_per_pix + b_scale*self.best_fit[1])**2)**1.5)
    denominator = np.absolute(2*a_scale*self.best_fit[0])

    self.radius_of_curvature = numerator/denominator

Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.

In [12]:
lane_images = glob.glob('output_images/lane_image_*.jpg')
lane_images.sort()
no_of_images = len(lane_images)

def putText(img, text, pos, font=cv2.FONT_HERSHEY_SIMPLEX, fontScale=2, fontColor=(255,255,255), lineType=2):
      cv2.putText(img, text, pos, font, fontScale, fontColor, lineType)  
    
# annotate an image with radius_of_curvature and line_base_pos for a lane
def annotate(img, left_line, right_line):
    if not left_line.detected or not right_line.detected:
        return
    
    radius_of_curvature = (left_line.radius_of_curvature + right_line.radius_of_curvature)/2
    putText(img, 'Radius of Curvature {0:.0f}(m)'.format(radius_of_curvature), (50, 60))
    
    center = (left_line.line_base_pos + right_line.line_base_pos)/2
    line_base_pos = center - left_line.line_base_pos
    leftRight = 'left' if line_base_pos > 0 else 'right' 
    putText(img, 'Vehicle is {0:.2f}m {1} of center'.format(abs(line_base_pos), leftRight), (50, 120))
    
for i in range(no_of_images):
#for i in [0]:
    img = mpimg.imread(lane_images[i])
    
    annotate(img, prior_lanes[i][0], prior_lanes[i][1])
    
    fname = lane_images[i].replace('lane_image_', 'annotate_');
    bgr_annotate_image = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
    cv2.imwrite(fname, bgr_annotate_image)
    
    # plot
    fig = plt.figure(figsize=(20, 20))
    fig.add_subplot(111)
    plt.imshow(img)
In [13]:
# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML
In [14]:
current_left_line = None
current_right_line = None
failed_attempts = 0
stats = {
    'fitSlidingWindow': 0,
    'fitAroundPoly': 0,
    'error': 0,
    'reset': 0,
    'missingLeftLine': 0,
    'missingRightLine': 0
}

def find_lane(img, max_failed_attempts):
    global current_left_line
    global current_right_line
    global failed_attempts
    global stats
    
    # the find lane pipeline consist of following steps
    
    # 1. resize to (1280, 720)
    scaled_img = cv2.resize(img, (1280, 720))
    
    # 2. undistort image
    undistort_img = cv2.undistort(scaled_img, mtx, dist, None, mtx)
    
    # 3. make binary image
    binary_img = combining_thresholds(
        undistort_img, 
        kernel=5, 
        x_thresh=(30, 255),
        y_thresh=(30, 255),
        mag_thresh=(50, 255),
        dir_thresh=(0.1, 1.4),
        s_thresh=(140, 255)
    )
    
    # 4. keep region of interest
    binary_img = region_of_interest(binary_img, vertices_roi)
    
    # 5. warp image to bird's eye view perspective
    warped_img = cv2.warpPerspective(binary_img, M, img_size, flags=cv2.INTER_LINEAR)
    
    # 6. find lane lines
    if current_left_line is None or current_right_line is None:
        stats['fitSlidingWindow'] += 1
        left_line, right_line = fitSlidingWindow(warped_img)
    else:
        stats['fitAroundPoly'] += 1
        left_line, right_line = fitAroundPoly(warped_img, current_left_line, current_right_line)
        
        # last chance before reset
        if (not left_line.detected or not right_line.detected) and (failed_attempts > max_failed_attempts - 1) :
            stats['fitSlidingWindow'] += 1
            current_left_line = None
            current_right_line = None
            left_line, right_line = fitSlidingWindow(warped_img)

    if sanityCheck(left_line, right_line) == True:
        
        if current_left_line is None:
            current_left_line = left_line
        else:
            current_left_line = current_left_line.append(left_line)
            
        if current_right_line is None:
            current_right_line = right_line
        else:
            current_right_line = current_right_line.append(right_line)
            
        failed_attempts = 0
    else:
        stats['error'] += 1
        failed_attempts += 1
        
        # max failed attempts - then reset current_left_line, current_right_line
        if failed_attempts > max_failed_attempts:
            stats['reset'] += 1
            current_left_line = None
            current_right_line = None
        if not left_line.detected:
             stats['missingLeftLine'] += 1
        if not right_line.detected:
             stats['missingRightLine'] += 1
            
    return current_left_line, current_right_line, undistort_img, binary_img, warped_img


# Do following line checks:
# - Checking is detected
# - Checking that they have similar curvature
# - Checking that they are separated by approximately the right distance horizontally
# - Checking that they are roughly parallel
def sanityCheck(left_line, right_line):
    max_reldiff_radius_of_curvature = 1.5
    max_delta_horizontally_distance = 0.3 # meter
    max_mse_roughly_parallel = 30 # pixels
    
    # Checking that they have similar curvature
    reldiff_radius_of_curvature = reldiff(left_line.radius_of_curvature, right_line.radius_of_curvature)
    
    # Checking that they are separated by approximately the right distance horizontally
    delta_horizontally_distance = abs(left_line.line_base_pos + right_line.line_base_pos - 3.7)
    
    # Checking that they are roughly parallel
    ploty = np.linspace(500, 699, 200) # bottom of image
    left_plotx = left_line.fitX(ploty)
    right_plotx = right_line.fitX(ploty)
    mse_roughly_parallel = ((right_plotx - left_plotx - 726)**2).mean()/200 # mean square error
    
    '''
    if reldiff_radius_of_curvature >= max_reldiff_radius_of_curvature:
        print('FAIL reldiff_radius_of_curvature', reldiff_radius_of_curvature)
    if delta_horizontally_distance >= max_delta_horizontally_distance:
        print('FAIL delta_horizontally_distance', delta_horizontally_distance)
    if mse_roughly_parallel >= max_mse_roughly_parallel:
        print('FAIL mse_roughly_parallel', mse_roughly_parallel)
    '''
    
    #print(left_line.best_fit, right_line.best_fit)
    return left_line.detected and right_line.detected and \
        reldiff_radius_of_curvature < max_reldiff_radius_of_curvature and \
        delta_horizontally_distance < max_delta_horizontally_distance and \
        mse_roughly_parallel < max_mse_roughly_parallel
    
def reldiff(a, b):
    return abs(a - b)/abs((a + b)/2)
    
def getErrorImage(image):
    scaled_image = cv2.resize(image, (1280, 720))
    undistort_image = cv2.undistort(scaled_image, mtx, dist, None, mtx)
    putText(undistort_image, 'ERROR!', (50, 60), fontColor=(255,0,0))
    return undistort_image
    
error = 0
def process_image(image):
    global error

    left_line, right_line, undistort_image, binary_image, warped_image = find_lane(image, 5)
    #temp_img = warped_img*255
    #return np.dstack((temp_img, temp_img, temp_img))

    # handle missing line case
    if left_line is None or right_line is None:
        error += 1
        error_image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR)
        cv2.imwrite('output_images/error_{}.jpg'.format(error), error_image)
        return getErrorImage(image)
        
    lane_image, lane_warped = drawLane(image, warped_image, Minv, left_line, right_line)
    annotate(lane_image, left_line, right_line)
    
    return lane_image
In [15]:
if not os.path.exists('videos_output'):
    os.makedirs('videos_output')
    
white_output = 'videos_output/project_video.mp4'

current_left_line = None
current_right_line = None
failed_attempts = 0
stats = {
    'fitSlidingWindow': 0,
    'fitAroundPoly': 0,
    'error': 0,
    'reset': 0,
    'missingLeftLine': 0,
    'missingRightLine': 0
}

clip1 = VideoFileClip("./project_video.mp4") #.subclip(13,15)
white_clip = clip1.fl_image(process_image)
%time white_clip.write_videofile(white_output, audio=False)
print(stats)
[MoviePy] >>>> Building video videos_output/project_video.mp4
[MoviePy] Writing video videos_output/project_video.mp4
100%|█████████▉| 1260/1261 [02:48<00:00,  7.49it/s]
[MoviePy] Done.
[MoviePy] >>>> Video ready: videos_output/project_video.mp4 

CPU times: user 11min 7s, sys: 46.9 s, total: 11min 53s
Wall time: 2min 48s
{'reset': 3, 'missingRightLine': 0, 'fitSlidingWindow': 4, 'missingLeftLine': 0, 'error': 77, 'fitAroundPoly': 1257}
In [16]:
HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(white_output))
Out[16]: